/*
* Copyright (C) 2024 Texas Instruments Incorporated
*
* All rights reserved not granted herein.
* Limited License.
*
* Texas Instruments Incorporated grants a world-wide, royalty-free,
* non-exclusive license under copyrights and patents it now or hereafter
* owns or controls to make, have made, use, import, offer to sell and sell ("Utilize")
* this software subject to the terms herein.  With respect to the foregoing patent
* license, such license is granted  solely to the extent that any such patent is necessary
* to Utilize the software alone.  The patent license shall not apply to any combinations which
* include this software, other than combinations with devices manufactured by or for TI ("TI Devices").
* No hardware patent is licensed hereunder.
*
* Redistributions must preserve existing copyright notices and reproduce this license (including the
* above copyright notice and the disclaimer and (if applicable) source code license limitations below)
* in the documentation and/or other materials provided with the distribution
*
* Redistribution and use in binary form, without modification, are permitted provided that the following
* conditions are met:
*
*	* No reverse engineering, decompilation, or disassembly of this software is permitted with respect to any
*     software provided in binary form.
*	* any redistribution and use are licensed by TI for use only with TI Devices.
*	* Nothing shall obligate TI to provide you with source code for the software licensed and provided to you in object code.
*
* If software source code is provided to you, modification and redistribution of the source code are permitted
* provided that the following conditions are met:
*
*   * any redistribution and use of the source code, including any resulting derivative works, are licensed by
*     TI for use only with TI Devices.
*   * any redistribution and use of any object code compiled from the source code and any resulting derivative
*     works, are licensed by TI for use only with TI Devices.
*
* Neither the name of Texas Instruments Incorporated nor the names of its suppliers may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* DISCLAIMER.
*
* THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
* BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL TI AND TI'S LICENSORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#ifdef GTRACK_3D

//system
#include <math.h>
#include <float.h>
// local
#include "gtrack_int.h"

/**
 *  @b Description
 *  @n
 *		This function is used to convert a vector from sherical to cartesian
 *
 *  @param[in]  v
 *		Pointer to measurements (spheriacl form) vector
 *  @param[out]  c
 *		Pointer to state vector (cartesian form)
 *
 *  \ingroup GTRACK_ALG_UTILITY_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_sph2cart(GTRACK_measurement_vector* v, GTRACK_cartesian_position* c)
{
    float sinAzim, cosAzim;
    float sinElev, cosElev;

    gtrack_sincosd(v->azimuth * RAD2DEG, &sinAzim, &cosAzim);
    gtrack_sincosd(v->elev * RAD2DEG, &sinElev, &cosElev);

    c->posX = v->range * cosElev * sinAzim;
    c->posY = v->range * cosElev * cosAzim;
    c->posZ = v->range * sinElev;
}
/**
 *  @b Description
 *  @n
 *		This function computes measurment error limits based on current target position and configured target dimensions
 *
 *  @param[in]  range
 *		target range
 *  @param[in]  gate_limits
 *		target dimentions limits
 *  @param[out]  limits
 *		Pointer to measurment error limits
 *
 *  \ingroup GTRACK_ALG_UTILITY_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_calcMeasurementLimits(float range, GTRACK_gateLimits* gate_limits, GTRACK_measurement_vector* limits)
{

    if (gate_limits->depth <= FLT_MIN)
        limits->range = FLT_MAX;
    else
        limits->range = gate_limits->depth / 2;

    if (gate_limits->width <= FLT_MIN)
        limits->azimuth = FLT_MAX;
    else
        limits->azimuth = atanf((gate_limits->width / 2) / range);

    if (gate_limits->height <= FLT_MIN)
        limits->elev = FLT_MAX;
    else
        limits->elev = atanf((gate_limits->height / 2) / range);

    if (gate_limits->vel <= FLT_MIN)
        limits->doppler = FLT_MAX;
    else
        limits->doppler = gate_limits->vel / 2;
}

void gtrack_calcMeasurementVar(GTRACK_measurement_vector* v, GTRACK_varParams* varP, GTRACK_measurement_vector* var)
{
    float azimErr;
    float elevErr;

    var->range = varP->depthStd * varP->depthStd;
    azimErr = 2 * atanf(0.5f * varP->widthStd / v->range);
    elevErr = 2 * atanf(0.5f * varP->heightStd / v->range);
    var->azimuth = azimErr * azimErr;
    var->elev = elevErr * elevErr;
    var->doppler = varP->dopplerStd * varP->dopplerStd;
}

/**
 *  @b Description
 *  @n
 *		This function computes target dimension estimations based on estimated measurement spread and centroid range
 *		Matrix is real, single precision floating point.
 *		Matrix is in row-major order
 *
 *  @param[in]  mSpread
 *		Vector S
 *  @param[in]  R
 *		scalar Range
 *  @param[out]  tDim
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_calcDim(float* mSpread, float R, float* tDim)
{
    tDim[0] = mSpread[0];
    tDim[1] = 2 * R * tanf(mSpread[1] / 2);
    tDim[2] = 2 * R * tanf(mSpread[2] / 2);
    tDim[3] = mSpread[3];
}

/**
 *  @b Description
 *  @n
 *		This function is used to calculate a distance between two points defined in measurement coordinates
 *
 *  @param[in]  p1
 *		Pointer to measurements (spheriacl form) vector
 *  @param[in]  p2
 *		Pointer to measurements (spheriacl form) vector
 *
 *  \ingroup GTRACK_ALG_UTILITY_FUNCTION
 *
 *  @retval
 *      distance, m
 */
float gtrack_calcDistance(GTRACK_measurement_vector* p1, GTRACK_measurement_vector* p2)
{
    // d = u1(1)^2 + u2(1)^2 - 2*u1(1)*u2(1)*(cos(u1(3))*cos(u2(3))*cos(u1(2)-u2(2))+sin(u1(3))*sin(u2(3)));
    float sinAzim, cosAzim;
    float sinElev1, cosElev1;
    float sinElev2, cosElev2;

    gtrack_sincosd((p1->azimuth - p2->azimuth) * RAD2DEG, &sinAzim, &cosAzim);
    gtrack_sincosd(p1->elev * RAD2DEG, &sinElev1, &cosElev1);
    gtrack_sincosd(p2->elev * RAD2DEG, &sinElev2, &cosElev2);

    return p1->range * p1->range + p2->range * p2->range - 2 * p1->range * p2->range * (cosElev1 * cosElev2 * cosAzim + sinElev1 * sinElev2);
}

/**
 *  @b Description
 *  @n
 *		This function computes Mahanalobis distance between vector v and distribution D.
 *		Vector is of length 4. Vector represents the delta between distribution centroid and measurment vector.
 *		Distribution is 4x4 matrix. Distribution represents the inverse of error covariance matrix.
 *		Vector is real, single precision floating point array.
 *		Matrix is real, single precision floating point array
 *		Matrix is in row-major order
 *
 *  @param[in]  v
 *		Vector v
 *  @param[in]  D
 *		Matrix D
 *  @param[out]  md
 *		md is computed 4 dimensional mahanalobis distance
 *		md = v*D*v';
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_computeMahalanobis(float* v, float* D, float* md)
{
    *md =
        v[0] * (v[0] * D[0] + v[1] * D[4] + v[2] * D[8] + v[3] * D[12]) +
        v[1] * (v[0] * D[1] + v[1] * D[5] + v[2] * D[9] + v[3] * D[13]) +
        v[2] * (v[0] * D[2] + v[1] * D[6] + v[2] * D[10] + v[3] * D[14]) +
        v[3] * (v[0] * D[3] + v[1] * D[7] + v[2] * D[11] + v[3] * D[15]);
}
/**
 *  @b Description
 *  @n
 *		This function computes partial Mahanalobis distance between vector v and distribution D.
 *		Vector is of length 4. Vector represents the delta between distribution centroid and measurment vector.
 *		The last dimension of vector is ignored (v[3] = 0).
 *		Distribution is 4x4 matrix. Distribution represents the inverse of error covariance matrix.
 *		Vector is real, single precision floating point array.
 *		Matrix is real, single precision floating point array
 *		Matrix is in row-major order
 *
 *  @param[in]  v
 *		Vector v
 *  @param[in]  D
 *		Matrix D
 *  @param[out]  mdp
 *		mdp is computed 3 dimensional mahanalobis distance (ignoring forth dimension, i.e v[3] = 0)
 *		v[3]=0; mdp = v*D*v';
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_computeMahalanobisPartial(float* v, float* D, float* mdp)
{
    *mdp =
        v[0] * (v[0] * D[0] + v[1] * D[4] + v[2] * D[8]) +
        v[1] * (v[0] * D[1] + v[1] * D[5] + v[2] * D[9]) +
        v[2] * (v[0] * D[2] + v[1] * D[6] + v[2] * D[10]);
}
/**
 *  @b Description
 *  @n
 *		This function is used to multiply two matrices.
 *		First matrix P is of size 9x9, the second one is of the size 4x9.
 *		The second matrix is being transposed first.
 *		Matrices are all real, single precision floating point.
 *		Matrices are in row-major order
 *
 *  @param[in]  P
 *		Matrix P
 *  @param[in]  J
 *		Matrix J
 *  @param[out]  PJ
 *		Matrix PJ = P(9,9) X J(4,9)T
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */

void gtrack_matrixComputePJT(float* P, float* J, float* PJ)
{
    /* We compute PJ' first because it is re-used in Kalman Gain calculations */
    uint16_t i, j, k, n;
    /* PJ[9x4] = P[9x9] x J[4x9]' */
    for (i = 0U, k = 0U; i < 36U; i += 4U, k += 9U)
    {
        for (j = 0U, n = 0U; j < 4U; j++, n += 9U)
        {
            PJ[i + j] = (P[k + 0U] * J[n + 0U]) +
                (P[k + 1U] * J[n + 1U]) +
                (P[k + 2U] * J[n + 2U]) +
                (P[k + 3U] * J[n + 3U]) +
                (P[k + 4U] * J[n + 4U]) +
                (P[k + 5U] * J[n + 5U]) +
                (P[k + 6U] * J[n + 6U]) +
                (P[k + 7U] * J[n + 7U]) +
                (P[k + 8U] * J[n + 8U]);
        }
    }
}
/**
 *  @b Description
 *  @n
 *		This function computes the inverse of 4x4 matrix.
 *		Matrix is real, single precision floating point.
 *		Matrix is in row-major order
 *
 *  @param[in]  A
 *		Matrix A
 *  @param[out]  det_out
 *		det_out = determinant;
 *  @param[out]  B
 *		B = inverse(A);
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */

void gtrack_matrixInv(const float* A, float* det_out, float* B)
{
    float inv[16];
    float det;
    float invdet;
    int   i;

    inv[0] = A[5] * A[10] * A[15] -
        A[5] * A[11] * A[14] -
        A[9] * A[6] * A[15] +
        A[9] * A[7] * A[14] +
        A[13] * A[6] * A[11] -
        A[13] * A[7] * A[10];

    inv[4] = -A[4] * A[10] * A[15] +
        A[4] * A[11] * A[14] +
        A[8] * A[6] * A[15] -
        A[8] * A[7] * A[14] -
        A[12] * A[6] * A[11] +
        A[12] * A[7] * A[10];

    inv[8] = A[4] * A[9] * A[15] -
        A[4] * A[11] * A[13] -
        A[8] * A[5] * A[15] +
        A[8] * A[7] * A[13] +
        A[12] * A[5] * A[11] -
        A[12] * A[7] * A[9];

    inv[12] = -A[4] * A[9] * A[14] +
        A[4] * A[10] * A[13] +
        A[8] * A[5] * A[14] -
        A[8] * A[6] * A[13] -
        A[12] * A[5] * A[10] +
        A[12] * A[6] * A[9];

    inv[1] = -A[1] * A[10] * A[15] +
        A[1] * A[11] * A[14] +
        A[9] * A[2] * A[15] -
        A[9] * A[3] * A[14] -
        A[13] * A[2] * A[11] +
        A[13] * A[3] * A[10];

    inv[5] = A[0] * A[10] * A[15] -
        A[0] * A[11] * A[14] -
        A[8] * A[2] * A[15] +
        A[8] * A[3] * A[14] +
        A[12] * A[2] * A[11] -
        A[12] * A[3] * A[10];

    inv[9] = -A[0] * A[9] * A[15] +
        A[0] * A[11] * A[13] +
        A[8] * A[1] * A[15] -
        A[8] * A[3] * A[13] -
        A[12] * A[1] * A[11] +
        A[12] * A[3] * A[9];

    inv[13] = A[0] * A[9] * A[14] -
        A[0] * A[10] * A[13] -
        A[8] * A[1] * A[14] +
        A[8] * A[2] * A[13] +
        A[12] * A[1] * A[10] -
        A[12] * A[2] * A[9];

    inv[2] = A[1] * A[6] * A[15] -
        A[1] * A[7] * A[14] -
        A[5] * A[2] * A[15] +
        A[5] * A[3] * A[14] +
        A[13] * A[2] * A[7] -
        A[13] * A[3] * A[6];

    inv[6] = -A[0] * A[6] * A[15] +
        A[0] * A[7] * A[14] +
        A[4] * A[2] * A[15] -
        A[4] * A[3] * A[14] -
        A[12] * A[2] * A[7] +
        A[12] * A[3] * A[6];

    inv[10] = A[0] * A[5] * A[15] -
        A[0] * A[7] * A[13] -
        A[4] * A[1] * A[15] +
        A[4] * A[3] * A[13] +
        A[12] * A[1] * A[7] -
        A[12] * A[3] * A[5];

    inv[14] = -A[0] * A[5] * A[14] +
        A[0] * A[6] * A[13] +
        A[4] * A[1] * A[14] -
        A[4] * A[2] * A[13] -
        A[12] * A[1] * A[6] +
        A[12] * A[2] * A[5];

    inv[3] = -A[1] * A[6] * A[11] +
        A[1] * A[7] * A[10] +
        A[5] * A[2] * A[11] -
        A[5] * A[3] * A[10] -
        A[9] * A[2] * A[7] +
        A[9] * A[3] * A[6];

    inv[7] = A[0] * A[6] * A[11] -
        A[0] * A[7] * A[10] -
        A[4] * A[2] * A[11] +
        A[4] * A[3] * A[10] +
        A[8] * A[2] * A[7] -
        A[8] * A[3] * A[6];

    inv[11] = -A[0] * A[5] * A[11] +
        A[0] * A[7] * A[9] +
        A[4] * A[1] * A[11] -
        A[4] * A[3] * A[9] -
        A[8] * A[1] * A[7] +
        A[8] * A[3] * A[5];

    inv[15] = A[0] * A[5] * A[10] -
        A[0] * A[6] * A[9] -
        A[4] * A[1] * A[10] +
        A[4] * A[2] * A[9] +
        A[8] * A[1] * A[6] -
        A[8] * A[2] * A[5];

    det = A[0] * inv[0] + A[1] * inv[4] + A[2] * inv[8] + A[3] * inv[12];

    invdet = 1.0f / det;

    for (i = 0; i < 16; i++)
        B[i] = inv[i] * invdet;

    *det_out = det;
}

/**
 *  @b Description
 *  @n
 *		This function computes the determinant of 4x4 matrix.
 *		Matrix is real, single precision floating point.
 *		Matrix is in row-major order
 *
 *  @param[in]  A
 *		Matrix A
 *  @param[out]  det
 *		det = det(A);
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_matrixDet(float* A, float* det)
{
    float inv[4];

    inv[0] = A[5] * A[10] * A[15] -
        A[5] * A[11] * A[14] -
        A[9] * A[6] * A[15] +
        A[9] * A[7] * A[14] +
        A[13] * A[6] * A[11] -
        A[13] * A[7] * A[10];

    inv[1] = -A[4] * A[10] * A[15] +
        A[4] * A[11] * A[14] +
        A[8] * A[6] * A[15] -
        A[8] * A[7] * A[14] -
        A[12] * A[6] * A[11] +
        A[12] * A[7] * A[10];

    inv[2] = A[4] * A[9] * A[15] -
        A[4] * A[11] * A[13] -
        A[8] * A[5] * A[15] +
        A[8] * A[7] * A[13] +
        A[12] * A[5] * A[11] -
        A[12] * A[7] * A[9];

    inv[3] = -A[4] * A[9] * A[14] +
        A[4] * A[10] * A[13] +
        A[8] * A[5] * A[14] -
        A[8] * A[6] * A[13] -
        A[12] * A[5] * A[10] +
        A[12] * A[6] * A[9];

    *det = A[0] * inv[0] + A[1] * inv[1] + A[2] * inv[2] + A[3] * inv[3];
}

/**
 *  @b Description
 *  @n
 *		This function checks whether measurement point P is geometrically behind the target
 *
 *  @param[in]  p
 *		Pointer to measurements (spheriacl form) vector
 *  @param[in]  target
 *		Pointer to measurements (spheriacl form) vector, representing target centroid
 *  @retval
 *      1 if behind, 0 is otherwise
 */
uint8_t gtrack_isPointBehindTarget(GTRACK_measurement_vector* p, GTRACK_measurement_vector* target)
{
    if ((fabsf(p->azimuth - target->azimuth) < 2 * PI / 180) &&
        (fabsf(p->elev - target->elev) < 2 * PI / 180) &&
        (p->range > target->range))
        return 1U;
    else
        return 0;
}

/**
 *  @b Description
 *  @n
 *		This function checks whether the point is inside the box boundary or not
 *
 *  @param[in]  c
 *		pointer to a position in cartesian space
 *  @param[in] box
 *		pointer to a 3D box object
 *
 *  \ingroup GTRACK_ALG_UTILITY_FUNCTION
 *
 *  @retval
 *      1 if inside, 0 otherwise
 */
uint8_t gtrack_isPointInsideBox(GTRACK_cartesian_position* c, GTRACK_boundaryBox* box)
{
    if ((c->posX > box->x1) && (c->posX < box->x2) &&
        (c->posY > box->y1) && (c->posY < box->y2) &&
        (c->posZ > box->z1) && (c->posZ < box->z2))
        return 1U;
    else
        return 0;
}
#endif
